﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Drawing;
using System.Diagnostics;
using MotionPlanning.MotionPlanningPrimitives;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems.CollisionPrimitives;

namespace MotionPlanning.MotionPlanningComponents.Visualizations
{
    class Visualizer_Impreza : Visualizer_Car1st
    {

        public Visualizer_Impreza(Graphics canvas, Rectangle rect)
            :base(canvas, rect)
        {
        }

        override public void PlotTrajectory(Trajectory trajectory)
        {
            System_SimplifiedPlanarPhysicalRobot system = (System_SimplifiedPlanarPhysicalRobot)planner_subject.GetSystem();
            RK4Helper pad = new RK4Helper(system.Get_DOF_state());
            this.DrawPhysicalEnvironment(system);
            if (trajectory == null)
                return;
            double[] q_new = new double[system.Get_DOF_state()];
            List<Edge> Edges = trajectory.GetEdges();
            double time_accu = 0;
            double time_delta = 0.5;
            double time_total = 0;

            int index_duration = planner_subject.GetSystem().Get_DOF_control();
            for (int i = 0; i < Edges.Count - 1; ++i)
            {
                time_total += Edges[i].get_Value().get_ctrl_inc()[index_duration];
            }

            double time_beginning_of_current_edge = 0;
            double time_end_of_current_edge = 0;
            int index_of_current_edge = 0;
            while (time_accu < time_total)
            {
                time_end_of_current_edge = time_beginning_of_current_edge + Edges[index_of_current_edge].get_Value().get_ctrl_inc()[index_duration];

                if (time_accu <= time_end_of_current_edge)
                {
                    double ratio = (time_accu - time_beginning_of_current_edge) / (time_end_of_current_edge - time_beginning_of_current_edge);

                    double[] q_start = Edges[index_of_current_edge].get_entry().get_Value().get_q();
                    double[] u_inc = Edges[index_of_current_edge].get_Value().get_ctrl_inc();
                    double t_inc = u_inc[index_duration] * ratio;
                    double cost_inc = 0;
                    planner_subject.GetSystem().Propagate(pad, q_start, u_inc, t_inc, ref q_new, ref cost_inc);
                    this.DrawPhysicalRobot(system, q_new); // this should be q_new

                    time_accu += time_delta;
                }
                else
                {
                    time_beginning_of_current_edge = time_end_of_current_edge;
                    index_of_current_edge++;
                }
            }
            this.DrawPhysicalRobot(system, Edges[Edges.Count-2].get_exit().get_Value().get_q()); // draw the last state
        }

        override public void DrawPhysicalRobot(System_SimplifiedPlanarPhysicalRobot env, double[] q)
        {
            List<Polygon_2D> objList = env.Get_Geometry_Robot();
            for (int i = 0; i < objList.Count; ++i)
            {
                List<double[]> current_obs = null;
                objList[i].TransformToEnvironment(q, ref current_obs);
                Point[] points = new Point[current_obs.Count];
                for (int j = 0; j < current_obs.Count; ++j)
                {
                    points[j].X = (int)(center_x + current_obs[j][0] * calibrater_x);
                    points[j].Y = (int)(center_y + current_obs[j][1] * calibrater_y);
                }

                canvas.FillPolygon(brushRobot, points);
                canvas.DrawLine(edgeWhite, points[0], points[1]);
                canvas.DrawLine(edgeWhite, points[1], points[2]);
                canvas.DrawLine(edgeWhite, points[2], points[3]);
                canvas.DrawLine(edgeWhite, points[3], points[0]);

                double robotCenter_x_left = (current_obs[0][0] + current_obs[1][0]) / 2;
                double robotCenter_y_left = (current_obs[0][1] + current_obs[1][1]) / 2;
                double robotCenter_x_top = (current_obs[0][0] + current_obs[3][0]) / 2;
                double robotCenter_y_top = (current_obs[0][1] + current_obs[3][1]) / 2;
                double robotCenter_x_bottom = (current_obs[1][0] + current_obs[2][0]) / 2;
                double robotCenter_y_bottom = (current_obs[1][1] + current_obs[2][1]) / 2;
                double robotCenter_x_center = (robotCenter_x_bottom + robotCenter_x_top) / 2;
                double robotCenter_y_center = (robotCenter_y_bottom + robotCenter_y_top) / 2;

                double robotCenter_x = robotCenter_x_left * 0.3333 + robotCenter_x_center * 0.6666;
                double robotCenter_y = robotCenter_y_left * 0.3333 + robotCenter_y_center * 0.6666;
                int robotCenter_x_screen = (int)(center_x + robotCenter_x * calibrater_x);
                int robotCenter_y_screen = (int)(center_y + robotCenter_y * calibrater_y);

                double slipd = ((System_Impreza)planner_subject.GetSystem()).Get_SlipVelocity(q);
                SolidBrush brush = brushGreen;
                if (Math.Abs(slipd) > 0.5)
                    brush = brushRed;
                canvas.FillEllipse(brush, robotCenter_x_screen - 3, robotCenter_y_screen - 3, 6, 6);
            }

        }

        override public void Set_ActiveArea_Primary(Control control)
        {
        }

        override protected void ActiveArea_primary_MouseMove(object sender, MouseEventArgs e)
        {
        }

        override protected void ActiveArea_primary_MouseUp(object sender, MouseEventArgs e)
        {
        }

        override protected void ActiveArea_primary_MouseDown(object sender, MouseEventArgs e)
        {
        }

    }
}
